#include "bt_manager/ros_interface.hpp"

namespace Sentry_BT
{
  RosInterface::RosInterface(ros::NodeHandle& nh, std::shared_ptr<Blackboard>& blackboard_ptr)
    : nav_client_("move_base", true)
    , blackboard_(blackboard_ptr)
  {
    // 订阅事件状态话题
    event_sub = nh.subscribe<robot_msgs::EventStatus>(
        "/sentry/event_status",
        1,
        &RosInterface::eventCallback, this);

    odom_sub = nh.subscribe<nav_msgs::Odometry>("/odom", 1,
        [this](const nav_msgs::Odometry::ConstPtr& msg)
        {
          // 更新当前位置
          current_pose_ = msg->pose.pose;
        });

    // 创建发布器
    outpost_pub = nh.advertise<std_msgs::Bool>("/sentry/outpost_status", 10);

    // 定时发布前哨站状态
    timer_ = nh.createTimer(ros::Duration(0.1),
        [this](const ros::TimerEvent&)
        {
          auto current_mode = blackboard_->get<int>("current_mode");
          std_msgs::Bool outpost_msg;
          if(current_mode == Sentry_BT::NavMode::RESPONSE &&
             std::hypot(current_pose_.position.x - nav_points[2].x, current_pose_.position.y - nav_points[2].y) < 1.0)
          {
            outpost_msg.data = true;
            outpost_pub.publish(outpost_msg);
          }
        });

    // 等待导航服务器可用
    ROS_INFO("等待导航服务器...");
    nav_client_.waitForServer();
    ROS_INFO("导航服务器已连接");
  }

  void RosInterface::eventCallback(const robot_msgs::EventStatus::ConstPtr& msg)
  {
    // 更新黑板中的数据
    blackboard_->set<float>("health", ((int)msg->self_health / 4));
    blackboard_->set<bool>("own_outpost_destroyed", msg->own_outpost_destroyed);
    blackboard_->set<int>("enemy_outpost_health", msg->enemy_outpost_health);
    blackboard_->set<bool>("bonus_active", msg->buff_active);
    blackboard_->set<bool>("target_valid", msg->enemy_detected.is_get);
   
    // 更新目标位置
    if(false)  // 原代码中为false，保持相同逻辑
    {
      geometry_msgs::Pose target_pose_in, target_pose;
      target_pose_in.position.x = (msg->enemy_detected.position.x) / 1000.0;  // 转换为米
      target_pose_in.position.y = (msg->enemy_detected.position.y) / 1000.0;
      target_pose_in.position.z = (msg->enemy_detected.position.z) / 1000.0;
      TransformPose(target_pose_in, target_pose);
      target_pose.orientation.w = 1.0;  // 设置默认朝向
      blackboard_->set<int>("target_armor_id", (int)msg->enemy_detected.armor_id);
      blackboard_->set<geometry_msgs::Pose>("target_pose", target_pose);
    }

    // ROS_INFO("EventStatus received: health=%.2f, outpost=%s, buff=%s, target_locked=%s",
    //          msg->self_health,
    //          msg->own_outpost_destroyed ? "true" : "false",
    //          msg->buff_active ? "true" : "false",
    //          msg->enemy_detected.is_get ? "true" : "false");
  }

  bool RosInterface::publishNavigationGoal(const Sentry_BT::Point2D& goal)
  {
    move_base_msgs::MoveBaseGoal goal_msg;
    goal_msg.target_pose.header.frame_id = "map";
    goal_msg.target_pose.header.stamp = ros::Time::now();
    goal_msg.target_pose.pose.position.x = goal.x;
    goal_msg.target_pose.pose.position.y = goal.y;
    goal_msg.target_pose.pose.orientation.w = 1.0;
    ROS_INFO("发送导航目标点: (%.2f, %.2f)", goal.x, goal.y);

    // 发送目标并设置完成回调
    nav_client_.sendGoal(goal_msg,
        [this](const actionlib::SimpleClientGoalState& state,
               const move_base_msgs::MoveBaseResult::ConstPtr& result)
        {
          (void)result;  // 避免未使用参数警告
          
          if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
          {
            ROS_INFO("导航成功");
            blackboard_->set("nav_status", static_cast<int>(Sentry_BT::NavStatus::IDLE));
          }
          else if(state == actionlib::SimpleClientGoalState::PREEMPTED)
          {
            ROS_INFO("导航被取消");
            blackboard_->set("nav_status", static_cast<int>(Sentry_BT::NavStatus::IDLE));
          }
          else if(state == actionlib::SimpleClientGoalState::ABORTED)
          {
            ROS_INFO("导航中止");
            blackboard_->set("nav_status", static_cast<int>(Sentry_BT::NavStatus::IDLE));
          }
          else
          {
            ROS_INFO("导航失败");
            blackboard_->set("nav_status", static_cast<int>(Sentry_BT::NavStatus::FAILURE));
          }
        });

    return true;
  }

  bool RosInterface::TransformPose(const geometry_msgs::Pose& input_pose, geometry_msgs::Pose& output_pose)
  {
    // 创建TransformUtils实例
    auto transform_utils = std::make_shared<Sentry_BT::TransformUtils>();

    // 执行坐标转换
    bool success = transform_utils->transformPoseToBaseLink(input_pose, output_pose);

    // if(success)
    // {
    //   ROS_INFO("坐标转换成功: (%.2f, %.2f) -> (%.2f, %.2f)",
    //            input_pose.position.x,
    //            input_pose.position.y,
    //            output_pose.position.x,
    //            output_pose.position.y);
    // }
    // else
    // {
    //   ROS_ERROR("坐标转换失败");
    //   output_pose = input_pose;  // 失败时返回原始坐标
    // }

    return success;
  }
}  // namespace Sentry_BT